/**
  ******************************************************************************
  * @file           : studio_full_path_coverage.cpp
  * @author         : wangyingjie
  * @brief          : None
  * @attention      : None
  * @date           : 2025/6/8
  ******************************************************************************
  */

#include "studio_wave_cover_ccpp.h"


// 设置电子围栏区域
void studio_wave_cover_ccpp::setFencedArea(const std::map<int, std::vector<int>>& all_block)
{
    for (const auto& [y, x_list] : all_block) // y 是行号 (y坐标)
    {
        for (int x : x_list) // x 是列号 (x坐标)
        {
            // 确保坐标在地图范围内
            if (x >= 0 && x < MAP_WIDTH && y >= 0 && y < MAP_HEIGHT)
            {
                Position pos(x, y);            // (x, y) 格式
                prob_map[pos].occupancy = 1.0; // 设置为可通行区域
            }
        }
    }
}

// 设置起点
void studio_wave_cover_ccpp::setStartPosition(Position start)
{
    start_position = start;
}

// 获取规划路径
std::vector<Position> studio_wave_cover_ccpp::getOptimalPath()
{
    return optimal_path;
}


std::vector<Position> studio_wave_cover_ccpp::getNeighbors(Position pos)
{
    int x = pos.first;
    int y = pos.second;
    std::vector<Position> neighbor_positions
    {
        {x - 1, y - 1}, {x, y - 1}, {x + 1, y - 1},
        {x - 1, y}, {x + 1, y},
        {x - 1, y + 1}, {x, y + 1}, {x + 1, y + 1}
    };

    // 返回包含所有邻居位置的向量
    return neighbor_positions;
}


void studio_wave_cover_ccpp::updateNeighbors(Position root_outosition)
{
    // 获取当前位置的邻居位置
    std::vector<Position> neighbor_positions = getNeighbors(root_outosition);
    // 遍历周围8个邻居位置
    for (auto& neighbor : neighbor_positions)
    {
        // 检查位置是否在地图范围内
        if (neighbor.first < 0 || neighbor.first >= MAP_WIDTH ||
            neighbor.second < 0 || neighbor.second >= MAP_HEIGHT)
        {
            continue;
        }

        // 只处理那些尚未计算成本且非障碍物的邻居位置
        if (prob_map[neighbor].occupancy != AO_INF)
        {
            if (!prob_map[neighbor].costComputed)
            {
                prob_map[neighbor].cost = prob_map[root_outosition].cost + 1;
                prob_map[neighbor].costComputed = true;
                candidate_positions.push_back(neighbor);
            }
        }
    }
}

/**
 * 使用波前算法传播成本
 *
 * 该函数从一个起始位置开始，逐步更新其周围位置的成本信息，直到所有可到达的位置都被更新
 * 它主要用于路径规划和成本计算
 *
 * @param start_position 起始位置，表示为二维坐标
 */
void studio_wave_cover_ccpp::waveFrontSpread(Position start_position)
{
    // 标记起始位置的成本为已计算
    prob_map[start_position].costComputed = true;
    // 初始化当前处理的位置为起始位置
    Position curr_position = start_position;
    // 更新起始位置的邻居位置的成本
    updateNeighbors(curr_position);
    // 当还有待处理的位置时，继续循环
    while (!candidate_positions.empty())
    {
        // 取出下一个待处理的位置
        curr_position = candidate_positions.front();
        // 从待处理列表中移除该位置
        candidate_positions.pop_front();
        // 更新当前位置的邻居位置的成本
        updateNeighbors(curr_position);
    }
}

/**
 * 重置地图上各位置的出口成本及相关状态
 *
 * 此函数遍历地图上的每一个位置，并重置与出口成本计算相关的状态信息
 * 包括是否检查过出口、出口成本、是否被追踪等属性这些状态信息
 * 对于路径规划和成本计算至关重要，通过重置这些值，可以准备
 * 对地图状态进行新一轮的评估或重置当前的评估结果
 */
void studio_wave_cover_ccpp::resetExitCost()
{
    for (int i = 0; i < MAP_WIDTH; i++)
    {
        for (int j = 0; j < MAP_HEIGHT; j++)
        {
            Position pos(i, j);
            prob_map[pos].exitChecked = false;
            prob_map[pos].exitCost = 0.0;
            prob_map[pos].isTracked = false;
        }
    }
    shortcut_candidate_positions.clear();
    sub_path.clear();
}

/**
 * 寻找出口位置
 *
 * @param stuck_position 被困的位置
 * @return Position 找到的出口位置，如果找不到则返回无穷远的坐标
 *
 * 此函数尝试从被困位置找到一个出口，通过检查周围8个邻居位置来确定
 * 是否找到了出口，或者是否有潜在的路径可以通往出口
 */
Position studio_wave_cover_ccpp::findExit(Position stuck_position)
{
    // 获取被困位置周围8个相邻格
    std::vector<Position> neighbor_positions = getNeighbors(stuck_position);
    Position exit_point = Position(AO_INF, AO_INF);
    for (auto& neighbor : neighbor_positions)
    {
        // 检查邻居位置是否在地图的有效范围内
        if (neighbor.first < 0 || neighbor.first >= MAP_WIDTH || neighbor.second < 0 || neighbor.second >= MAP_HEIGHT)
        {
            continue;
        }
        // 如果邻居位置不是障碍物且未被访问过，则将其标记为出口，并记录前一个位置
        if (prob_map[neighbor].occupancy != AO_INF && !prob_map[neighbor].isVisited)
        {
            isExitFound = true;
            prob_map[neighbor].prev_point_position = stuck_position;
            // 更新出口点为当前邻居位置，并终止搜索
            exit_point = neighbor;
            break;
        }
        // 如果邻居位置不是障碍物且未检查过出口，则将其标记为已检查出口，并计算出口成本
        else if (prob_map[neighbor].occupancy != AO_INF && !prob_map[neighbor].exitChecked)
        {
            prob_map[neighbor].exitChecked = true;
            prob_map[neighbor].exitCost = prob_map[stuck_position].exitCost + 1;
            prob_map[neighbor].prev_point_position = stuck_position;
            // 将当前邻居位置添加到潜在捷径位置列表中
            shortcut_candidate_positions.push_back(neighbor);
        }
    }

    return exit_point;
}

/**
 * 当机器人被困在角落时，尝试找到逃脱路径的函数
 * 使用 Dijkstra 算法从角落逃离。
 * @param stuck_position 机器人被困的位置
 */
void studio_wave_cover_ccpp::cornerEscape(Position stuck_position)
{
    // 标记当前位置已检查出口，并开始跟踪此位置
    prob_map[stuck_position].exitChecked = true;
    prob_map[stuck_position].isTracked = true;

    // 初始化当前位置为被困位置，并尝试找到出口
    Position curr_position = stuck_position;
    Position found_point = findExit(curr_position);

    // 当存在候选快捷位置时，循环检查每个位置以找到出口
    while (!shortcut_candidate_positions.empty())
    {
        // 获取并移除候选快捷位置列表中的第一个位置
        curr_position = shortcut_candidate_positions.front();
        shortcut_candidate_positions.pop_front();

        // 尝试从当前候选快捷位置找到出口
        found_point = findExit(curr_position);

        // 如果找到了有效的出口位置
        if (found_point.first != AO_INF && found_point.second != AO_INF)
        {
            // 从出口位置回溯到起始位置，标记路径上的每个位置为已访问和跟踪
            Position position = found_point;
            while (!(prob_map[position].prev_point_position.first == AO_INF &&
                     prob_map[position].prev_point_position.second == AO_INF) &&
                   !prob_map[position].isTracked)
            {
                prob_map[position].isVisited = true;
                prob_map[position].visitedNum++;
                prob_map[position].isTracked = true;
                // 将回溯路径添加到子路径中
                sub_path.push_front(position);

                // 如果前一个位置是起点，则停止回溯
                if (prob_map[position].prev_point_position == stuck_position)
                {
                    break;
                }
                position = prob_map[position].prev_point_position;
            }

            // 将子路径添加到最优路径中
            optimal_path.insert(optimal_path.end(), sub_path.begin(), sub_path.end());
            // 重置出口找到标志和相关成本
            isExitFound = false;
            resetExitCost();
            return;
        }
    }

    // 如果所有候选快捷位置都检查完毕仍未找到出口，则规划完成
    isPlanningFinished = true;
    std::cout << "Path planning finished." << std::endl;
}

/**
 * 寻找下一步的最佳位置
 *
* 检查当前位置的 8 个邻居
    跳过：地图外位置 / 障碍物 / 已访问点
    选择 成本最小 (min_cost) 的未访问邻居，如果无法找到更优位置，则调用角落逃脱函数
 *
 * @param position 当前位置，表示为(std::pair<int, int>)，其中first和second分别表示行和列
 */
void studio_wave_cover_ccpp::findNextStep(Position position)
{
    // 初始化是否被困的状态，用于判断是否需要调用角落逃脱函数
    bool isStuck = true;
    // 初始化最小代价为无穷大，用于比较找到的更优位置
    double min_cost = AO_INF;
    // 初始化下一个位置为当前位置，如果没有找到更优位置，则不会改变
    Position next_position = position;

    // 获取当前位置的所有邻居位置
    std::vector<Position> neighbor_positions = getNeighbors(position);

    // 遍历所有邻居位置，寻找最优的下一步位置
    for (auto& neighbor : neighbor_positions)
    {
        // 检查位置是否在地图范围内
        if (neighbor.first < 0 || neighbor.first >= MAP_WIDTH ||
            neighbor.second < 0 || neighbor.second >= MAP_HEIGHT)
        {
            continue;
        }

        // 忽略已经被访问过或者代价为无穷大的邻居位置
        if ((prob_map[neighbor].occupancy != AO_INF) && (!prob_map[neighbor].isVisited))
        {
            // 如果邻居位置的代价小于当前最小代价，则更新最小代价和下一个位置
            if (prob_map[neighbor].cost < min_cost)
            {
                min_cost = prob_map[neighbor].cost;
                next_position = neighbor;
                // 找到了更优的位置，因此不再是被困状态
                isStuck = false;
            }
        }
    }

    // 如果没有找到更优的位置，调用角落逃脱函数
    if (isStuck)
    {
        cornerEscape(position);
    }
    else
    {
        // 如果找到了更优的位置，标记为已访问并增加访问次数，同时添加到最优路径中
        prob_map[next_position].isVisited = true;
        prob_map[next_position].visitedNum++;
        optimal_path.push_back(next_position);
    }
}

/**
 * 执行完整覆盖路径规划算法
 *
 * 该函数从给定的起始位置开始，利用波前展开法进行路径规划，直到路径规划完成
 * 它通过在概率地图上标记访问过的区域并计算访问次数来帮助机器人覆盖所有区域
 */
void studio_wave_cover_ccpp::planPath()
{
    if (start_position.first == AO_INF || start_position.second == AO_INF)
    {
        std::cerr << "Error: Start position not set!" << std::endl;
        return;
    }

    // 利用波前展开法初始化路径规划
    waveFrontSpread(start_position);

    // 标记起始位置为已访问，并增加访问次数
    prob_map[start_position].isVisited = true;
    prob_map[start_position].visitedNum++;
    // 将起始位置添加到最优路径列表中
    optimal_path.push_back(start_position);

    // 初始化当前位置为起始位置
    Position curr_position = start_position;

    // 循环直到路径规划完成
    while (!isPlanningFinished)
    {
        // 寻找当前位置的下一步最佳位置
        findNextStep(curr_position);
        // 更新当前位置为路径的最后一个位置
        if (!optimal_path.empty())
        {
            curr_position = optimal_path.back();
        }
    }
}